{
  "cells": [
    {
      "cell_type": "markdown",
      "metadata": {
        "id": "intro_md"
      },
      "source": [
        "# GeneralSFMFactor"
      ]
    },
    {
      "cell_type": "markdown",
      "metadata": {
        "id": "desc_md"
      },
      "source": [
        "This header defines factors for Structure from Motion (SfM) problems where camera calibration might be unknown or optimized alongside pose and structure.\n",
        "\n",
        "`GeneralSFMFactor<CAMERA, LANDMARK>`:\n",
        "- A binary factor connecting a `CAMERA` variable and a `LANDMARK` variable.\n",
        "- Represents the reprojection error of the `LANDMARK` into the `CAMERA` view, compared to a 2D `measured_` pixel coordinate.\n",
        "- The `CAMERA` type encapsulates both pose and calibration (e.g., `PinholeCamera<Cal3Bundler>`).\n",
        "- Error: `camera.project(landmark) - measured`\n",
        "\n",
        "`GeneralSFMFactor2<CALIBRATION>`:\n",
        "- A ternary factor connecting a `Pose3` variable, a `Point3` landmark variable, and a `CALIBRATION` variable.\n",
        "- This explicitly separates the camera pose and calibration into different variables.\n",
        "- Error: `PinholeCamera<CALIBRATION>(pose, calibration).project(landmark) - measured`\n",
        "\n",
        "These factors are core components for visual SLAM or SfM systems where calibration is refined or initially unknown."
      ]
    },
    {
      "cell_type": "markdown",
      "metadata": {
        "id": "colab_badge_md"
      },
      "source": [
        "<a href=\"https://colab.research.google.com/github/borglab/gtsam/blob/develop/gtsam/slam/doc/GeneralSFMFactor.ipynb\" target=\"_parent\"><img src=\"https://colab.research.google.com/assets/colab-badge.svg\" alt=\"Open In Colab\"/></a>"
      ]
    },
    {
      "cell_type": "code",
      "execution_count": null,
      "metadata": {
        "id": "pip_code",
        "tags": [
          "remove-cell"
        ]
      },
      "outputs": [],
      "source": [
        "%pip install --quiet gtsam-develop"
      ]
    },
    {
      "cell_type": "code",
      "execution_count": 1,
      "metadata": {
        "id": "imports_code"
      },
      "outputs": [],
      "source": [
        "import gtsam\n",
        "import numpy as np\n",
        "from gtsam import (GeneralSFMFactorCal3_S2, GeneralSFMFactor2Cal3_S2,\n",
        "                   PinholeCameraCal3_S2, Pose3, Point3, Point2, Cal3_S2, Values)\n",
        "from gtsam import symbol_shorthand\n",
        "\n",
        "X = symbol_shorthand.X\n",
        "L = symbol_shorthand.L\n",
        "K = symbol_shorthand.K\n",
        "C = symbol_shorthand.C # For Camera variable"
      ]
    },
    {
      "cell_type": "markdown",
      "metadata": {
        "id": "factor1_header_md"
      },
      "source": [
        "## 1. `GeneralSFMFactor<CAMERA, LANDMARK>`"
      ]
    },
    {
      "cell_type": "markdown",
      "metadata": {
        "id": "factor1_desc_md"
      },
      "source": [
        "Connects a combined Camera variable (pose+calibration) and a Landmark.\n",
        "Requires the `Values` object to contain instances of the specific `CAMERA` type (e.g., `PinholeCameraCal3_S2`)."
      ]
    },
    {
      "cell_type": "code",
      "execution_count": 2,
      "metadata": {
        "id": "factor1_example_code"
      },
      "outputs": [
        {
          "name": "stdout",
          "output_type": "stream",
          "text": [
            "GeneralSFMFactor:   keys = { c0 l0 }\n",
            "  noise model: unit (2) \n",
            "GeneralSFMFactor: .z[\n",
            "\t320;\n",
            "\t240\n",
            "]\n",
            "\n",
            "Error for GeneralSFMFactor: 0.0\n"
          ]
        }
      ],
      "source": [
        "measured_pt = Point2(320, 240) # Measurement in pixels\n",
        "sfm_noise = gtsam.noiseModel.Isotropic.Sigma(2, 1.0)\n",
        "camera_key = C(0)\n",
        "landmark_key = L(0)\n",
        "\n",
        "# Note: The factor type name includes the Calibration, e.g., GeneralSFMFactorCal3_S2\n",
        "factor1 = GeneralSFMFactorCal3_S2(measured_pt, sfm_noise, camera_key, landmark_key)\n",
        "factor1.print(\"GeneralSFMFactor: \")\n",
        "\n",
        "# Evaluate error - requires a Camera object in Values\n",
        "values = Values()\n",
        "camera_pose = Pose3() # Identity pose\n",
        "calibration = Cal3_S2(500, 500, 0, 320, 240) # fx, fy, s, u0, v0\n",
        "camera = PinholeCameraCal3_S2(camera_pose, calibration)\n",
        "landmark = Point3(0, 0, 5) # Point 5m in front of camera\n",
        "\n",
        "values.insert(camera_key, camera)\n",
        "values.insert(landmark_key, landmark)\n",
        "\n",
        "error1 = factor1.error(values)\n",
        "print(f\"\\nError for GeneralSFMFactor: {error1}\") # Should be [0, 0] if landmark projects to measured_pt"
      ]
    },
    {
      "cell_type": "markdown",
      "metadata": {
        "id": "factor2_header_md"
      },
      "source": [
        "## 2. `GeneralSFMFactor2<CALIBRATION>`"
      ]
    },
    {
      "cell_type": "markdown",
      "metadata": {
        "id": "factor2_desc_md"
      },
      "source": [
        "Connects separate Pose3, Point3 (Landmark), and Calibration variables."
      ]
    },
    {
      "cell_type": "code",
      "execution_count": 3,
      "metadata": {
        "id": "factor2_example_code"
      },
      "outputs": [
        {
          "name": "stdout",
          "output_type": "stream",
          "text": [
            "GeneralSFMFactor2:   keys = { x0 l0 k0 }\n",
            "  noise model: unit (2) \n",
            "GeneralSFMFactor2: .z[\n",
            "\t320;\n",
            "\t240\n",
            "]\n",
            "\n",
            "Error for GeneralSFMFactor2: 0.0\n"
          ]
        }
      ],
      "source": [
        "pose_key = X(0)\n",
        "calib_key = K(0)\n",
        "# landmark_key = L(0) # Re-use from above\n",
        "\n",
        "# Note: The factor type name includes the Calibration, e.g., GeneralSFMFactor2Cal3_S2\n",
        "factor2 = GeneralSFMFactor2Cal3_S2(measured_pt, sfm_noise, pose_key, landmark_key, calib_key)\n",
        "factor2.print(\"GeneralSFMFactor2: \")\n",
        "\n",
        "# Evaluate error - requires Pose3, Point3, Cal3_S2 objects in Values\n",
        "values2 = Values()\n",
        "values2.insert(pose_key, camera_pose)\n",
        "values2.insert(landmark_key, landmark)\n",
        "values2.insert(calib_key, calibration)\n",
        "\n",
        "error2 = factor2.error(values2)\n",
        "print(f\"\\nError for GeneralSFMFactor2: {error2}\")"
      ]
    }
  ],
  "metadata": {
    "kernelspec": {
      "display_name": "gtsam",
      "language": "python",
      "name": "python3"
    },
    "language_info": {
      "codemirror_mode": {
        "name": "ipython",
        "version": 3
      },
      "file_extension": ".py",
      "mimetype": "text/x-python",
      "name": "python",
      "nbconvert_exporter": "python",
      "pygments_lexer": "ipython3",
      "version": "3.13.1"
    }
  },
  "nbformat": 4,
  "nbformat_minor": 0
}
